#just the sensor infomation
module Sensormodule

def ruby_maplaserbayes_stand(robot,distr,maptype)
   angbench=Benchmark.measure{
    if(@usetrue==true) 
    robotx=robot.truepos[0].to_f
    roboty=robot.truepos[1].to_f
    robotang=robot.truepos[5].to_f
    else
    robotx=robot.ins[0].to_f
    roboty=robot.ins[1].to_f
    robotang=robot.ins[5].to_f
    end
    oldcell=Array.new()
    scans=robot.rangescan.size
    #check the integraty of the data
    
    if(!robotang.nil? and !robotx.nil? and !roboty.nil? and scans>0)    
    robot.rangescan.each_with_index{|member,rad|
       range=member.to_f
       laserdist=self.standarddist(range,distr)
       if(range<19.9)
         #log the before the wall  
         frequency=(19.9 / maptype.cellwidth).to_i #log till the end of the sensor model
         frequency.times{|sub_range|
         range=sub_range*maptype.cellwidth
         degree=0.0174*(scans-rad)
         x=range*Math.cos(degree+(robotang.to_f-(3.1415/2)))
         y=range*Math.sin(degree+(robotang.to_f-(3.1415/2)))
         cellpos=maptype.converttocell(x+robotx.to_f,y+roboty.to_f)
         oldprob=maptype.getmap(cellpos[0],cellpos[1]).to_f
         sensprob=laserdist[(range*@laserscale).to_i] 
         sensprob=0.99 if sensprob>1.0
         if(sensprob==0.0)
           break
         end
           if(oldcell!=cellpos)
              newvalue=self.bayesupdate(oldprob,sensprob)  
              maptype.putmap(cellpos[0],cellpos[1],newvalue) if(newvalue<1.0 and newvalue>0.0)
           end
           oldcell=Array.new(cellpos)
         } 
       
       
    
       end
     
          
     }
    end   
        
   }
   bench_array=angbench.to_a
   puts "angbench= #{(bench_array[1].to_f+bench_array[2].to_f)}" if(@debug==1)
   #calculate average time
   @total=@total+(bench_array[1].to_f+bench_array[2].to_f)
   @timescalled+=1.0
   @avetime=@total/@timescalled
   puts "avetime:#{@avetime}"
end
  
  
#update the map using the step function as the sensor model
def ruby_maplaserbayes(robot,maptype)
   angbench=Benchmark.measure{
    if(@usetrue==true) 
    robotx=robot.truepos[0].to_f
    roboty=robot.truepos[1].to_f
    robotang=robot.truepos[5].to_f
    else
    robotx=robot.ins[0].to_f
    roboty=robot.ins[1].to_f
    robotang=robot.ins[5].to_f
    end
    scans=robot.rangescan.size
    #check the integraty of the data
    if(!robotang.nil? and !robotx.nil? and !roboty.nil? and scans>0)    
    robot.rangescan.each_with_index{|member,rad|
       range=member.to_f
       
       if(range<19.9)
         #log the white spaces
         frequency=(range/maptype.cellwidth).to_i
         frequency.times{|sub_range|
           range=sub_range*maptype.cellwidth
           degree=0.0174*(scans-rad)
           x=range*Math.cos(degree+(robotang.to_f-(3.1415/2)))
           y=range*Math.sin(degree+(robotang.to_f-(3.1415/2)))
           cellpos=maptype.converttocell(x+robotx.to_f,y+roboty.to_f)
           oldprob=maptype.getmap(cellpos[0],cellpos[1]).to_f
           newvalue=self.bayesupdate(oldprob,0.43)
           maptype.putmap(cellpos[0],cellpos[1],newvalue) if(newvalue>0.0)
         } 
    
         #log the wall and 2 points behind it
         2.times{|point|
         range=member.to_f+(point*maptype.cellwidth)
         degree=0.0174*(scans-rad)
         x=range*Math.cos(degree+(robotang.to_f-(3.1415/2)))
         y=range*Math.sin(degree+(robotang.to_f-(3.1415/2)))
         cellpos=maptype.converttocell(x+robotx.to_f,y+roboty.to_f)
         oldprob=maptype.getmap(cellpos[0],cellpos[1]).to_f
         newvalue=self.bayesupdate(oldprob,0.60)
         if(newvalue<1.0)
         2.times{|y|
           2.times{|x|
              maptype.putmap(cellpos[0]+(x-1),cellpos[1]+(y-1),newvalue) 
           }
         }
         end
         }
       end
     
          
     }
    end   
        
   }
   bench_array=angbench.to_a
   puts "angbench= #{(bench_array[1].to_f+bench_array[2].to_f)}" if(@debug==1)
end  
  
def ruby_maplaserincrement(robot,maptype)
    angbench=Benchmark.measure{
    increment=0.05
    if(@usetrue==true) 
      robotx=robot.truepos[0].to_f
      roboty=robot.truepos[1].to_f
      robotang=robot.truepos[5].to_f
    else
      robotx=robot.ins[0].to_f
      roboty=robot.ins[1].to_f
      robotang=robot.ins[5].to_f
    end
    oldcell=Array.new()
    scans=robot.rangescan.size
    #check the integraty of the data
    if(!robotang.nil? and !robotx.nil? and !roboty.nil? and scans>0)    
    robot.rangescan.each_with_index{|member,rad|
       range=member.to_f
       if(range<19.9)
         #log the white spaces
         frequency=(range/maptype.cellwidth).to_i
         frequency.times{|sub_range|
           range=sub_range*maptype.cellwidth
           degree=0.0174*(scans-rad)
           x=range*Math.cos(degree+(robotang.to_f-(3.1415/2)))
           y=range*Math.sin(degree+(robotang.to_f-(3.1415/2)))
           cellpos=maptype.converttocell(x+robotx.to_f,y+roboty.to_f)
           oldprob=maptype.getmap(cellpos[0],cellpos[1]).to_f
           if(oldcell!=cellpos)
           maptype.putmap(cellpos[0],cellpos[1],oldprob-increment) if(oldprob+increment>0.0)
           end
           oldcell=Array.new(cellpos)
         } 
       
         #log the wall and 2 points behind it
         2.times{|point|
         range=member.to_f+(point*maptype.cellwidth)
         degree=0.0174*(scans-rad)
         x=range*Math.cos(degree+(robotang.to_f-(3.1415/2)))
         y=range*Math.sin(degree+(robotang.to_f-(3.1415/2)))
         cellpos=maptype.converttocell(x+robotx.to_f,y+roboty.to_f)
         oldprob=maptype.getmap(cellpos[0],cellpos[1]).to_f
         maptype.putmap(cellpos[0],cellpos[1],oldprob+increment) if(oldprob-increment<1.0)
         }
       end
     
          
     }
    end   
        
   }
   bench_array=angbench.to_a
   puts "angbench= #{(bench_array[1].to_f+bench_array[2].to_f)}" if(@debug==1)
 end
 
 
  def ruby_maplaseroverwrite(robot,maptype)
   angbench=Benchmark.measure{
    scans=robot.rangescan.size
    #check the integraty of the data
    if(!robot.ins[5].nil? and !robot.ins[0].nil? and !robot.ins[1].nil? and scans>0)    
    robot.rangescan.each_with_index{|member,rad|
       range=member.to_f
      
       if(range<19.9)
         #log the white spaces
         frequency=(range/maptype.cellwidth).to_i
         frequency.times{|sub_range|
           range=sub_range*maptype.cellwidth
           degree=0.0174*(scans-rad)
           x=range*Math.cos(degree+(robot.ins[5].to_f-(3.1415/2)))
           y=range*Math.sin(degree+(robot.ins[5].to_f-(3.1415/2)))
           cellpos=maptype.converttocell(x+robot.ins[0].to_f,y+robot.ins[1].to_f)
           maptype.putmap(cellpos[0],cellpos[1],0.0)
         } 
       
         #log the wall
         range=member.to_f
         degree=0.0174*(scans-rad)
         x=range*Math.cos(degree+(robot.ins[5].to_f-(3.1415/2)))
         y=range*Math.sin(degree+(robot.ins[5].to_f-(3.1415/2)))
         cellpos=maptype.converttocell(x+robot.ins[0].to_f,y+robot.ins[1].to_f)
         maptype.putmap(cellpos[0],cellpos[1],1.0)
       end
     
          
     }
    end   
        
   }
   bench_array=angbench.to_a
   puts "angbench= #{(bench_array[1].to_f+bench_array[2].to_f)}" if(@debug==1)
   
  end
  
end